#ifndef STEERING_DRIVE_H
#define STEERING_DRIVE_H

#include "motor_controller.h"

// 舵机转向后轮驱动实现
class SteeringDrive : public MotorController {
private:
  int motorL1, motorL2, motorR1, motorR2;
  int steeringPin;
  int maxSpeed;
  int currentAngle;

public:
  SteeringDrive(int l1, int l2, int r1, int r2, int steerPin) {
    motorL1 = l1;
    motorL2 = l2;
    motorR1 = r1;
    motorR2 = r2;
    steeringPin = steerPin;
    maxSpeed = 255;
    currentAngle = 90; // 初始居中
  }

  void init() override {
    pinMode(motorL1, OUTPUT);
    pinMode(motorL2, OUTPUT);
    pinMode(motorR1, OUTPUT);
    pinMode(motorR2, OUTPUT);
    pinMode(steeringPin, OUTPUT);
    setSteeringAngle(90); // 居中
    stop();
  }

  void setSteeringAngle(int angle) {
    angle = constrain(angle, 45, 135); // 限制角度范围
    currentAngle = angle;
    int pulseWidth = map(angle, 0, 180, 500, 2500);
    analogWrite(steeringPin, pulseWidth / 4); // 转换为0-255范围
  }

  void forward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, speed);
    analogWrite(motorL2, 0);
    analogWrite(motorR1, speed);
    analogWrite(motorR2, 0);
  }

  void backward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    analogWrite(motorL1, 0);
    analogWrite(motorL2, speed);
    analogWrite(motorR1, 0);
    analogWrite(motorR2, speed);
  }

  void turnLeft(int speed) override {
    setSteeringAngle(45); // 左转
    forward(speed);
  }

  void turnRight(int speed) override {
    setSteeringAngle(135); // 右转
    forward(speed);
  }

  void stop() override {
    analogWrite(motorL1, 0);
    analogWrite(motorL2, 0);
    analogWrite(motorR1, 0);
    analogWrite(motorR2, 0);
  }

  void setSpeed(int speed) override {
    maxSpeed = constrain(speed, 0, 255);
  }

  String getType() override {
    return "SteeringDrive";
  }
};

#endif // STEERING_DRIVE_H